/*
wt901c485.c
offer sensor driver realize function
designed by lunanting

-----------------------
2021-02-19 14:00:29
create file
-----------------------

*/

#include "wt901c485.h"
#include "../../bsp/modbus_485.h"
#include "../../bsp/bsp.h"
#include "stdlib.h"



static int xp_wt901c_debug(char *type,char *fun,char *param);
static int debug_add=0;

/*
wt901c sensor init
param:
    port:uart port
    speed:buard speed
    id:slave id
return: the object pointer
    NULL==init fail~
*/
WT901C *xp_wt901c_init(int port,int speed,int id){
    WT901C *wt;
    int sta=0;

    if(id<1||id>127){
        perr("init fail,id set error~ id=%d",id);
        return NULL;
    }
    wt=(WT901C *)malloc(sizeof(WT901C));
    memset(wt,0,sizeof(WT901C));
    wt->port=port;
    wt->speed=speed;
    wt->id=id;
    wt->gMax=16;
    wt->degMax=2000;
    wt->freq=10;
    sta=xp_modbus_init(port,speed,MB_PAR_NONE,100);
    if(sta!=0){
        perr("init fail,modbus init fail~");
        wt->init=0;
        return NULL;
    }
    if(debug_add==0){
        if(xp_cli_debug_add(xp_wt901c_debug)!=0){
            perr("add wt901c debug fail~");
            return NULL;
        }
        debug_add=1;
    }

    println("wt901c sensor id=%d init success~",id);
    wt->init=1;
    return wt;
}



/**************************************read data***************************/
/*
read data
param:
    obj:object
    reg_addr: register address
    len:read lenght
    rec_buf:save recived data;
    read_len:recived lenght
return:
    0=success
*/
int xp_wt901c_read(WT901C *obj,u16 reg_addr,u16 len,u8 *rec_buf,u8 *rec_len){
    MB_data modbus;
    u8 count=5;

    if(obj==NULL){
        perr("object == NULL");
        return -1;
    }
    modbus.slave_id=obj->id;
    modbus.mb_fun=mb_hold_r;
    modbus.start_addr=reg_addr;
    modbus.rwLen=len;
    modbus.resBuf=rec_buf;
    modbus.resLen=rec_len;
    while(count--){
        if(xp_modbus_ask(obj->port,&modbus)==0)return 0;
    }
    perr("read wt901c fail~");
    return -1;
}




/**************************************set config*************************/
/*
set config
param:
    obj:object
    reg_addr:register address
    data: set data
return:
    0=success
*/
int xp_wt901c_set(WT901C *obj,u16 reg_addr,u16 data){
    MB_data modbus;
    u8 rec_data[10];
    u8 rec_len;
    u8 count=5;
    u16 dt=data;

    if(obj==NULL){
        perr("object == NULL");
        return -1;
    }
    modbus.slave_id=obj->id;
    modbus.mb_fun=mb_single_w;
    modbus.start_addr=reg_addr;
    modbus.wBuf=&dt;
    modbus.resBuf=rec_data;
    modbus.resLen=&rec_len;
    while(count--){
        if(xp_modbus_ask(obj->port,&modbus)==0)return 0;
    }
    perr("wt901c write fail~");
    return -1;
}



/*********************************congig**************************************/
/*
config sensor
param:
    obj:sensor
    type:algorithm type, 1=6轴 or 0=9轴  算法类型，6轴还是9轴
    dir: put direction，安装方向，0=水平安装，1=垂直安装
    gyro_adjust:1=关闭自动，0=开启自动
return:
     0=success
*/
int xp_wt901c_config(WT901C *obj,int type,int dir,int gyro_adjust){
    if(xp_wt901c_set(obj,ADDR_algorithm,type)!=0)return -1;
    if(xp_wt901c_set(obj,ADDR_dir,dir)!=0)return -2;
    if(xp_wt901c_set(obj,ADDR_gyro_auto,gyro_adjust)!=0)return -3;
    if(xp_wt901c_set(obj,ADDR_adjust,0x0007)!=0)return -4;
    aos_msleep(2000);
    if(xp_wt901c_set(obj,ADDR_adjust,0x0000)!=0)return -5;
    aos_msleep(500);
    if(xp_wt901c_set(obj,ADDR_restart,0x0000)!=0)return -6;
    aos_msleep(1000);
    return 0;
} 


/*
config max deg
param:
    type:max deg type
return:
    0=success
*/
int xp_wt901c_set_deg(WT901C *obj,WT901C_max_deg type){
    u16 data;
    switch (type)
    {
    case wt901c_deg_250:
        data=0x0000;
        obj->degMax=250;
        break;
    case wt901c_deg_500:
        data=0x0001;
        obj->degMax=500;
        break;
    case wt901c_deg_1000:
        data=0x0002;
        obj->degMax=1000;
        break;
    case wt901c_deg_2000:
        data=0x0003;
        obj->degMax=2000;
        break;
    default:
        data=0x0003;
        obj->degMax=2000;
        break;
    }
    if(xp_wt901c_set(obj,ADDR_deg_rang,data)!=0)return -1;
    return 0;
}

/*
config max acc
param:
    type:max acc type
return:
    0=success
*/
int xp_wt901c_set_acc(WT901C *obj,WT901C_max_acc type){
    u16 data;
    switch (type)
    {
    case wt901c_acc_2g:
        data=0x0000;
        obj->gMax=2;
        break;
    case wt901c_acc_4g:
        data=0x0001;
        obj->gMax=4;
        break;
    case wt901c_acc_8g:
        data=0x0002;
        obj->gMax=8;
        break;
    case wt901c_acc_16g:
        data=0x0003;
        obj->gMax=16;
        break;
    default:
        data=0x0003;
        obj->gMax=16;
        break;
    }
    if(xp_wt901c_set(obj,ADDR_acc_rang,data)!=0)return -1;
    return 0;
}

/*
config freq
param:
    type:freq type
return:
    0=success
*/
int xp_wt901c_set_freq(WT901C *obj,WT901C_freq type){
    u16 data;
    switch (type)
    {
    case wt901c_freq_256hz:
        data=0x0000;
        obj->freq=256;
        break;
    case wt901c_freq_184hz:
        data=0x0001;
        obj->freq=184;
        break;
    case wt901c_freq_94hz:
        data=0x0002;
        obj->freq=94;
        break;
    case wt901c_freq_44hz:
        data=0x0003;
        obj->freq=44;
        break;
    case wt901c_freq_21hz:
        data=0x0004;
        obj->freq=21;
        break;
    case wt901c_freq_10hz:
        data=0x0005;
        obj->freq=10;
        break;
    case wt901c_freq_5hz:
        data=0x0006;
        obj->freq=5;
        break;
    default:
        data=0x0005;
        obj->freq=10;
        break;
    }
    if(xp_wt901c_set(obj,ADDR_freq,data)!=0)return -1;
    return 0;
}

/*
config baud speed
param:
    type: baud speed type
return:
    0=success
*/
int xp_wt901c_set_baud(WT901C *obj,WT901C_baud type){
    u16 data;

    if(type==wt901c_baud_2400){
        data=0x0000;
    }
    else if(type==wt901c_baud_4800){
        data=0x0001;
    }
    else if(type==wt901c_baud_9600){
        data=0x0002;
    }
    else if(type==wt901c_baud_19200){
        data=0x0003;
    }
    else if(type==wt901c_baud_38400){
        data=0x0004;
    }
    else if(type==wt901c_baud_57600){
        data=0x0005;
    }
    else if(type==wt901c_baud_115200){
        data=0x0006;
    }
    else if(type==wt901c_baud_230400){
        data=0x0007;
    }
    else if(type==wt901c_baud_460800){
        data=0x0008;
    }
    else if(type==wt901c_baud_921600){
        data=0x0009;
    }
    else{
        data=0x0006;
    }
    if(xp_wt901c_set(obj,ADDR_baud,data)!=0)return -1;
    return 0;
}


/*
set modbus id
param:
    obj:sensor
    id:set id
return:
    0=success
*/
int xp_wt901c_set_id(WT901C *obj,int id){
    u16 i=(id&0x00ff);
    if(xp_wt901c_set(obj,ADDR_id,i)!=0)return -1;
    return 0;
}



/**********************************************read data**********************************/
/*
read sensor
param:
    obj: sensor
    type:read sensor data type
return:
    0=success
*/
int xp_wt901c_read_sensor(WT901C *obj,WT901C_data_type type){
    u16 addr,read_len;
    u8 buf[20];
    u8 rec_len;
    int sta=0;
    s16 data;

    if(type==wt901c_data_time){
        addr=ADDR_time;
        read_len=0x0003;
        if(xp_wt901c_read(obj,addr,read_len,buf,&rec_len)!=0){sta= -1;goto END;}
    }
    else if(type==wt901c_data_acc){
        addr=ADDR_acc;
        read_len=0x0003;
        if(xp_wt901c_read(obj,addr,read_len,buf,&rec_len)!=0){sta= -2;goto END;}
        //计算方法
        data=(buf[0]<<8)|buf[1];
        obj->acc.x=data/V_value*obj->gMax*G_value;
        data=(buf[2]<<8)|buf[3];
        obj->acc.y=data/V_value*obj->gMax*G_value;
        data=(buf[4]<<8)|buf[5];
        obj->acc.z=data/V_value*obj->gMax*G_value;

    }
    else if(type==wt901c_data_deg){
        addr=ADDR_deg;
        read_len=0x0003;
        if(xp_wt901c_read(obj,addr,read_len,buf,&rec_len)!=0){sta= -3;goto END;}
        //计算方法
        data=(buf[0]<<8)|buf[1];
        obj->deg.x=data/V_value*obj->degMax;
        data=(buf[2]<<8)|buf[3];
        obj->deg.y=data/V_value*obj->degMax;
        data=(buf[4]<<8)|buf[5];
        obj->deg.z=data/V_value*obj->degMax;

    }
    else if(type==wt901c_data_angle){
        addr=ADDR_angle;
        read_len=0x0003;
        if(xp_wt901c_read(obj,addr,read_len,buf,&rec_len)!=0){sta= -4;goto END;}
        //计算方法
        data=(buf[0]<<8)|buf[1];
        obj->angle.x=data/V_value*180;  //roll
        obj->angle.rel_x=obj->angle.x-obj->angle.offset_x;
        if(obj->angle.rel_x<-180.0){
            obj->angle.rel_x+=360.0;
        }
        obj->angle.round_x=obj->angle.rel_x;
        if(obj->angle.round_x<0){
            obj->angle.round_x=360+obj->angle.round_x;
        }

        data=(buf[2]<<8)|buf[3];
        obj->angle.y=data/V_value*180;  //pitch
        obj->angle.rel_y=obj->angle.y-obj->angle.offset_y;
        if(obj->angle.rel_y<-180.0){
            obj->angle.rel_y+=360.0;
        }
        obj->angle.round_y=obj->angle.rel_y;
        if(obj->angle.round_y<0){
            obj->angle.round_y=360+obj->angle.round_y;
        }
        data=(buf[4]<<8)|buf[5];
        obj->angle.z=data/V_value*180;  //yaw
        obj->angle.rel_z=obj->angle.z-obj->angle.offset_z;
        if(obj->angle.rel_z<-180.0){
            obj->angle.rel_z+=360.0;
        }
        obj->angle.round_z=obj->angle.rel_z;
        if(obj->angle.round_z<0){
            obj->angle.round_z=360+obj->angle.round_z;
        }

    }
    else if(type==wt901c_data_mag){
        addr=ADDR_mag;
        read_len=0x0004;
        if(xp_wt901c_read(obj,addr,read_len,buf,&rec_len)!=0){sta= -5;goto END;}
        //计算方法
        obj->mag.x=(buf[0]<<8)|buf[1];
        obj->mag.y=(buf[2]<<8)|buf[3];
        obj->mag.z=(buf[4]<<8)|buf[5];

    }
    else if(type==wt901c_data_ph){
        addr=ADDR_high;
        read_len=0x0004;
        if(xp_wt901c_read(obj,addr,read_len,buf,&rec_len)!=0){sta= -6;goto END;}
    }
    else if(type==wt901c_data_port){
        addr=ADDR_port;
        read_len=0x0004;
        if(xp_wt901c_read(obj,addr,read_len,buf,&rec_len)!=0){sta= -7;goto END;}
    }
    else if(type==wt901c_data_q){
        addr=ADDR_q;
        read_len=0x0004;
        if(xp_wt901c_read(obj,addr,read_len,buf,&rec_len)!=0){sta= -8;goto END;}
        //计算方法
        data=(buf[3]<<8)|buf[4];
        obj->qua.q0=data/V_value;
        data=(buf[5]<<8)|buf[6];
        obj->qua.q1=data/V_value;
        data=(buf[7]<<8)|buf[8];
        obj->qua.q2=data/V_value;
        data=(buf[9]<<8)|buf[10];
        obj->qua.q3=data/V_value;

    }
    else if(type==wt901c_data_l){
        addr=ADDR_l;
        read_len=0x0004;
        if(xp_wt901c_read(obj,addr,read_len,buf,&rec_len)!=0){sta= -9;goto END;}
    }
    else{
        perr("no mathed sensor data type");
        return -10;
    }
    
    END:
    if(sta!=0){
        perr("sensor read data fail~ id=%d,type=%d",obj->id,type);
    }
    return sta;
}

/*
set angle zero
return:
    0=success
*/
int xp_wt901c_set_zero(WT901C *obj){
    //Z轴角度归0
    if(xp_wt901c_set(obj,ADDR_adjust,0x0004)!=0){
        perr("set zero fail~ id=%d",obj->id);
        return -1;
    }
    //读取偏移角度并记录
    if(xp_wt901c_read_sensor(obj,wt901c_data_angle)!=0){
        perr("read angle fail~ id=%d",obj->id);
        return -1;
    }
    obj->angle.offset_x=obj->angle.x;
    obj->angle.offset_y=obj->angle.y;
    obj->angle.offset_z=obj->angle.z;
    obj->angle.rel_x=0;
    obj->angle.rel_y=0;
    obj->angle.rel_z=0;
    obj->angle.round_x=0;
    obj->angle.round_y=0;
    obj->angle.round_z=0;
    println("%d sensor set zero success~ offset[%f %f %f]",obj->id, 
        obj->angle.offset_x, obj->angle.offset_y, obj->angle.offset_z);
}




/***************************************debug*********************************/
static WT901C *demo=NULL;
static void xp_wt901c_demo_update(void *param);

/*
demo init
return:
    0=success
*/
int xp_wt910c_demo_init(void){
    demo=xp_wt901c_init(1,115200,0x50);
    if(demo==NULL){
        return -1;
    }

    if (0 != aos_task_new("gyro_wt901c", xp_wt901c_demo_update, NULL, 1024)) {
        printf("create gyro_wt901c thread fail");
        return -2;
    }

    return 0;
}

int xp_wt910c_demo2_init(void){
    demo=xp_wt901c_init(1,19200,0x50);
    if(demo==NULL){
        return -1;
    }

    if (0 != aos_task_new("gyro_wt901c", xp_wt901c_demo_update, NULL, 1024)) {
        printf("create gyro_wt901c thread fail");
        return -2;
    }

    return 0;
}


/*
clear demo z angle
return:  angle
*/
int xp_wt901c_demo_zAngle_clear(void){
    int ret = -1;

    ret = xp_wt901c_set_zero(demo);
    return ret;
}

/*
get demo z angle
return:  angle
*/
float xp_wt901c_demo_zAngle_get(void){
    //逆时针角度变大
    return demo->angle.round_z;
}

static void xp_wt901c_demo_update(void *param)
{
    while(1) {
		xp_wt901c_read_sensor(demo,wt901c_data_angle);      
		aos_msleep(100);
    }
}

//wt901c debug
static int xp_wt901c_debug(char *type,char *fun,char *param){
    if(strcmp(type,"wt901c")!=0)return 0;
    if(strcmp(fun,"get_angle")==0){
        if(xp_wt901c_read_sensor(demo,wt901c_data_angle)==0){
            println("angle: x=%0.2f,    y=%0.2f,     z=%0.2f",demo->angle.round_x,demo->angle.round_y,demo->angle.round_z);
        }
    }
    else if(strcmp(fun,"set_acc")==0){
        xp_wt901c_set_acc(demo,atoi(param));
    }
    else if(strcmp(fun,"set_freq")==0){
        xp_wt901c_set_freq(demo,atoi(param));
    }
    else if(strcmp(fun,"set_deg")==0){
        xp_wt901c_set_deg(demo,atoi(param));
    }
    else if(strcmp(fun,"set_baud")==0){
        xp_wt901c_set_baud(demo,atoi(param));
    }
    else if(strcmp(fun,"set_zero")==0){
        xp_wt901c_set_zero(demo);
    }

    else{
        return 0;
    }
    return 1;
}


//end of the file
